
/**
  ******************************************************************************
  * Copyright 2021 The Microbee Authors. All Rights Reserved.
  * 
  * Licensed under the Apache License, Version 2.0 (the "License");
  * you may not use this file except in compliance with the License.
  * You may obtain a copy of the License at
  * 
  * http://www.apache.org/licenses/LICENSE-2.0
  * 
  * Unless required by applicable law or agreed to in writing, software
  * distributed under the License is distributed on an "AS IS" BASIS,
  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
  * See the License for the specific language governing permissions and
  * limitations under the License.
  * 
  * @file       mb_follow.h
  * @author     baiyang
  * @date       2023-8-9
  ******************************************************************************
  */

#pragma once

#ifdef __cplusplus
extern "C"{
#endif

/*----------------------------------include-----------------------------------*/
#include <pid/p.h>

#include <gcs_mavlink/gcs.h>
#include <common/gp_defines.h>
#include <rtc/jitter_correction.h>
#include <common/location/location.h>
#include <common/gp_math/gp_mathlib.h>
/*-----------------------------------macro------------------------------------*/

/*----------------------------------typedef-----------------------------------*/
// enum for FOLLOW_OPTIONS parameter
enum FollowOption {
    FOLLOW_MOUNT_FOLLOW_ON_ENTER = 1
};

// enum for YAW_BEHAVE parameter
enum FollowYawBehave {
    FOLLOW_YAW_BEHAVE_NONE = 0,
    FOLLOW_YAW_BEHAVE_FACE_LEAD_VEHICLE = 1,
    FOLLOW_YAW_BEHAVE_SAME_AS_LEAD_VEHICLE = 2,
    FOLLOW_YAW_BEHAVE_DIR_OF_FLIGHT = 3
};

typedef struct mb_l1_control* mb_follow_t;
typedef const struct mb_l1_control* mb_follow_const_t;

struct mb_follow {
    // parameters
    Param_int8     _enabled;           // 1 if this subsystem is enabled
    Param_int16    _sysid;             // target's mavlink system id (0 to use first sysid seen)
    Param_float    _dist_max;          // maximum distance to target.  targets further than this will be ignored
    Param_int8     _offset_type;       // offset frame type (0:North-East-Down, 1:RelativeToLeadVehicleHeading)
    Param_Vector3f _offset;            // offset from lead vehicle in meters
    Param_int8     _yaw_behave;        // following vehicle's yaw/heading behaviour (see YAW_BEHAVE enum)
    Param_int8     _alt_type;          // altitude source for follow mode
    P_ctrl         _p_pos;             // position error P controller
    Param_int16    _options;           // options for mount behaviour follow mode

    // local variables
    uint32_t       _last_location_update_ms;  // system time of last position update
    Location       _target_location;          // last known location of target
    Vector3f_t     _target_velocity_ned;      // last known velocity of target in NED frame in m/s
    Vector3f_t     _target_accel_ned;         // last known acceleration of target in NED frame in m/s/s
    uint32_t       _last_heading_update_ms;   // system time of last heading update
    float          _target_heading;           // heading in degrees
    bool           _automatic_sysid;          // did we lock onto a sysid automatically?
    float          _dist_to_target;           // latest distance to target in meters (for reporting purposes)
    float          _bearing_to_target;        // latest bearing to target in degrees (for reporting purposes)
    bool           _offsets_were_zero;        // true if offsets were originally zero and then initialised to the offset from lead vehicle

    // setup jitter correction with max transport lag of 3s
    JitterCorrection _jitter;
};
/*----------------------------------variable----------------------------------*/

/*-------------------------------------os-------------------------------------*/

/*----------------------------------function----------------------------------*/
void mb_follow_ctor();
bool mb_follow_enabled();
void mb_follow_set_target_sysid(uint8_t sysid);
uint8_t mb_follow_get_target_sysid();
const P_ctrl *mb_follow_get_pos_p();
void mb_follow_clear_offsets_if_required();
bool mb_follow_get_target_location_and_velocity(Location *loc, Vector3f_t *vel_ned);
bool mb_follow_get_velocity_ned(Vector3f_t *vel_ned, float dt);
bool mb_follow_get_target_dist_and_vel_ned(Vector3f_t *dist_ned, Vector3f_t *dist_with_offs, Vector3f_t *vel_ned);
void mb_follow_clear_dist_and_bearing_to_target();
void mb_follow_init_offsets_if_required(const Vector3f_t *dist_vec_ned);
void mb_follow_rotate_vector(Vector3f_t *vec2,const Vector3f_t *vec, float angle_deg);
void mb_follow_rotate_vector2(Vector3f_t *vec2, const float x, const float y, const float z, float angle_deg);
bool mb_follow_get_offsets_ned(Vector3f_t *offset);
enum FollowYawBehave mb_follow_get_yaw_behave();
bool mb_follow_get_target_heading_deg(float *heading);
bool mb_follow_get_target_location_and_velocity_ofs(Location *loc, Vector3f_t *vel_ned);
bool mb_follow_have_target(void);
void mb_follow_handle_msg(const mavlink_message_t *msg);
float mb_follow_get_distance_to_target();
float mb_follow_get_bearing_to_target();
uint32_t mb_follow_get_last_update_ms();
bool mb_follow_option_is_enabled(enum FollowOption option);
/*------------------------------------test------------------------------------*/

#ifdef __cplusplus
}
#endif



